#include "scanning.h"
using namespace std;

Scanning::Scanning():ArAction("actionScanning")
{
	avanti=0;
	gira=120;
	turn =0;
	//updt = 1;
	//t2 = clock()+DELAY;
}

Scanning::~Scanning()
{
}

ArActionDesired* Scanning::fire(ArActionDesired currentDesired)
{
	cout << "\nScansione in corso...\n";
	//myDesired.reset();
	/*if(avanti<100){
		myDesired.setVel(100);
		avanti=avanti+1;
		gira = 0;
		cout<<"sto andado avanti!!!\n";
	}
	if(avanti>=100){
		myDesired.setVel(0);
		myRobot->setDeltaHeading(10);
		gira = gira+1;
		cout<<"sto girando!!!\n";
	}
	if(gira>=100){
		avanti = 0;
	}*/
	if(gira<120){
		//turn = turn+0.3;
		myDesired.setVel(1);
	                myDesired.setDeltaHeading(5,1);
	                gira++;
	                cout << "giro "<<gira<<endl;

	        }else{
	                if(myDesired.getVel()!=400){
	                		myDesired.setDeltaHeading(0);
	                        myDesired.setVel(200);
	                }
	                avanti++;
	                cout << "cont1 (avanti) "<<avanti<<endl;
	                if(avanti==100){
	                        gira=0;
	                        avanti=0;
	                        turn =0;
	                }
	        }


	/*myDesired.reset();

	if(cont<12){
		myDesired.setVel(1);
		myDesired.setDeltaHeading(10);
		//myDesired.setVel(1);
		//myDesired.setDeltaHeading(10);
		cont++;
		cout << "cont (giro) "<<cont<<endl;
	}else{
		currentDesired.setDeltaHeading(0,0);

		if(myDesired.getVel()!=100 && cont1<12){
			myDesired.setVel(150);
			cont1++;
			cout << "cont1 (avanti) "<<cont1<<endl;
		}
		if(cont1>=12){
			cont=0;
			cont1=0;
		}
	}

	*/
	/*srand(time(NULL));
	int goOrTurn = rand()%100;
	int degree = 20;
	myDesired.reset();
	if(goOrTurn > 60){
		myDesired.setVel(250);
	}else{
		myDesired.setVel(0);
		myDesired.setDeltaHeading(degree,0.50);
	}*/

	/*
	if(updt == 1){
		t2 = clock()+DELAY;
		t3 = t2+DELAY/3;
		updt = 0;
	}
	t1 = clock();
	myDesired.reset();
	if(t1<t2){
		myDesired.setVel(1);
		myDesired.setDeltaHeading(5);
	}else{
		myDesired.setVel(250);
		if(t1>=t3){
			updt=1;
		}
	}*/

	return &myDesired;
}
